// Copyright 2022 Chen Jun
// Licensed under the MIT License.

#ifndef ARMOR_DETECTOR_ARMOR_HPP_
#define ARMOR_DETECTOR_ARMOR_HPP_

#include <Eigen/Eigen>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>

// STL
#include <algorithm>
#include <string>

namespace hnurm
{
const int RED  = 0;
const int BLUE = 1;

enum ArmorType
{
    SMALL = 0,
    LARGE = 1
};

struct Light : public cv::RotatedRect
{
    Light() = default;

    explicit Light(const cv::RotatedRect &box) : cv::RotatedRect(box)
    {

        box.points(p);
        std::sort(p, p + 4, [](const cv::Point2f &a, const cv::Point2f &b) { return a.y < b.y; });
        top    = (p[0] + p[1]) / 2;
        bottom = (p[2] + p[3]) / 2;

        length = cv::norm(top - bottom);
        width  = cv::norm(p[0] - p[1]);

        tilt_angle = std::atan2(std::abs(top.x - bottom.x), std::abs(top.y - bottom.y));
        tilt_angle = tilt_angle / CV_PI * 180;
    }

    int         color;
    cv::Point2f top, bottom;
    double      length;
    double      width;
    float       tilt_angle;
    cv::Point2f p[4];
};

struct Armor
{
    Armor() = default;

    Armor(const Light &l1, const Light &l2)
    {
        if(l1.center.x < l2.center.x)
        {
            left_light = l1, right_light = l2;
        }
        else
        {
            left_light = l2, right_light = l1;
        }
        center = (left_light.center + right_light.center) / 2;

        points2d.resize(9);
        points2d[0] = left_light.top;
        points2d[1] = left_light.bottom;
        points2d[2] = right_light.bottom;
        points2d[3] = right_light.top;

        points2d[4] = (left_light.top+right_light.top)/2;
        points2d[5] = (left_light.top+left_light.bottom)/2;
        points2d[6] = (left_light.bottom+right_light.bottom)/2;
        points2d[7] = (right_light.bottom+right_light.top)/2;

        points2d[8] = center;

    }

    void showArmor(cv::Mat &canvas)
    {
        // draw armor
        cv::line(canvas, points2d[0], points2d[1], cv::Scalar(0, 255, 0), 2);
        cv::line(canvas, points2d[1], points2d[2], cv::Scalar(0, 255, 0), 2);
        cv::line(canvas, points2d[2], points2d[3], cv::Scalar(0, 255, 0), 2);
        cv::line(canvas, points2d[3], points2d[0], cv::Scalar(0, 255, 0), 2);
        // draw center
        cv::circle(canvas, center, 10, cv::Scalar(0, 255, 0), 2);
        // draw classification res on left top corner of armor
        cv::putText(canvas, classification_result, points2d[0], cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2);
    }

    Light                    left_light, right_light;
    cv::Point2f              center;
    std::vector<cv::Point2f> points2d;
    cv::Mat                  number_img;

    std::string number;
    int         idx {};
    float       similarity {};
    float       confidence {};
    std::string classification_result;
    ArmorType   armor_type = ArmorType::SMALL;

    double distance_to_image_center {};
    // todo
    Eigen::Vector3d position;
    Eigen::Matrix3d rotation;
    Eigen::Matrix3d _r1;  // 相机坐标系到云台坐标系
    Eigen::Matrix3d _r2;  // 先对pitch进行旋转
    Eigen::Matrix3d _r3;  // 再对yaw的旋转,变为世界坐标系（pitch、yaw为0）
    Eigen::Matrix3d _R;   //本体系到相机系
    Eigen::Matrix3d _rmat;
    Eigen::Vector3d _translation;
};

}  // namespace hnurm

#endif  // ARMOR_DETECTOR_ARMOR_HPP_
